{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "import random\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_Rate=0.0000001\n",
    "Velocity_tripod=0.289*40\n",
    "Velocity_quadruped=0.115*40\n",
    "Velocity_onebyone=0.096*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120]) #\n",
    "Robot_r=40 #the radius of hexapod robot\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the map, 'RGB' mode is used to reprensent different colors.\n",
    "\n",
    "(0,0,0)---Black---Obstacle and margin\n",
    "\n",
    "(255,255,255)---White---Flat ground\n",
    "\n",
    "(255,0,0)---Red---Destination\n",
    "\n",
    "(0,255,255)---Cyan---Hexapod robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAmkAAAGXCAYAAADoAccmAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nQEIaAMCA/j8yeqPi2/wVTQAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x1de78bbfef0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def Draw_map4(Destination_x,Destination_y):\n",
    "    im4=Image.new('RGB',size=(3000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im4,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,1000,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2000,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,250,800,500),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2500,200,2700,1000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1100,300,2000,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2200,1550,2700,1850),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im4\n",
    "im4=Draw_map4(1750,500)\n",
    "plt.imshow(im4)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "States=np.array(['L60D0/30/H0','L180D0/30/H0','L250D0/30/H0','L300D0/30/H0',\n",
    "'L60D30/60/H0','L180D30/60/H0','L250D30/60/H0','L300D30/60/H0',\n",
    "'L60D60/90/H0','L180D60/90/H0','L250D60/90/H0','L300D60/90/H0',\n",
    "'L60D90/120/H0','L180D90/120/H0','L250D90/120/H0','L300D90/120/H0',\n",
    "'L60D120/150/H0','L180D120/150/H0','L250D120/150/H0','L300D120/150/H0',\n",
    "'L60D150/180/H0','L180D150/180/H0','L250D150/180/H0','L300D150/180/H0',\n",
    "'L60D180/210/H0','L180D180/210/H0','L250D180/210/H0','L300D180/210/H0',\n",
    "'L60D210/240/H0','L180D210/240/H0','L250D210/240/H0','L300D210/240/H0',\n",
    "'L60D240/270/H0','L180D240/270/H0','L250D240/270/H0','L300D240/270/H0',\n",
    "'L60D270/300/H0','L180D270/300/H0','L250D270/300/H0','L300D270/300/H0',\n",
    "'L60D300/330/H0','L180D300/330/H0','L250D300/330/H0','L300D300/330/H0',\n",
    "'L60D330/360/H0','L180D330/360/H0','L250D330/360/H0','L300D330/360/H0',\n",
    "'L60D0/30/H1','L180D0/30/H1','L250D0/30/H1','L300D0/30/H1',\n",
    "'L60D30/60/H1','L180D30/60/H1','L250D30/60/H1','L300D30/60/H1',\n",
    "'L60D60/90/H1','L180D60/90/H1','L250D60/90/H1','L300D60/90/H1',\n",
    "'L60D90/120/H1','L180D90/120/H1','L250D90/120/H1','L300D90/120/H1',\n",
    "'L60D120/150/H1','L180D120/150/H1','L250D120/150/H1','L300D120/150/H1',\n",
    "'L60D150/180/H1','L180D150/180/H1','L250D150/180/H1','L300D150/180/H1',\n",
    "'L60D180/210/H1','L180D180/210/H1','L250D180/210/H1','L300D180/210/H1',\n",
    "'L60D210/240/H1','L180D210/240/H1','L250D210/240/H1','L300D210/240/H1',\n",
    "'L60D240/270/H1','L180D240/270/H1','L250D240/270/H1','L300D240/270/H1',\n",
    "'L60D270/300/H1','L180D270/300/H1','L250D270/300/H1','L300D270/300/H1',\n",
    "'L60D300/330/H1','L180D300/330/H1','L250D300/330/H1','L300D300/330/H1',\n",
    "'L60D330/360/H1','L180D330/360/H1','L250D330/360/H1','L300D330/360/H1',\n",
    "'L60D0/30/H2','L180D0/30/H2','L250D0/30/H2','L300D0/30/H2',\n",
    "'L60D30/60/H2','L180D30/60/H2','L250D30/60/H2','L300D30/60/H2',\n",
    "'L60D60/90/H2','L180D60/90/H2','L250D60/90/H2','L300D60/90/H2',\n",
    "'L60D90/120/H2','L180D90/120/H2','L250D90/120/H2','L300D90/120/H2',\n",
    "'L60D120/150/H2','L180D120/150/H2','L250D120/150/H2','L300D120/150/H2',\n",
    "'L60D150/180/H2','L180D150/180/H2','L250D150/180/H2','L300D150/180/H2',\n",
    "'L60D180/210/H2','L180D180/210/H2','L250D180/210/H2','L300D180/210/H2',\n",
    "'L60D210/240/H2','L180D210/240/H2','L250D210/240/H2','L300D210/240/H2',\n",
    "'L60D240/270/H2','L180D240/270/H2','L250D240/270/H2','L300D240/270/H2',\n",
    "'L60D270/300/H2','L180D270/300/H2','L250D270/300/H2','L300D270/300/H2',\n",
    "'L60D300/330/H2','L180D300/330/H2','L250D300/330/H2','L300D300/330/H2',\n",
    "'L60D330/360/H2','L180D330/360/H2','L250D330/360/H2','L300D330/360/H2'])\n",
    "Actions=np.array(['FrontH0','FrontH1','FrontH2','Left_45D','Right_45D'])\n",
    "Length_state=len(States)\n",
    "Length_action=len(Actions)\n",
    "Q_table=np.zeros((Length_state,Length_action))\n",
    "Q_table=pd.DataFrame(Q_table,columns=Actions,index=States)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Initial_Q_Table(Length_action,Length_state):\n",
    "    Q_Table=np.zeros((Length_state,Length_action))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Set_destination(x_range,y_range):\n",
    "    x=np.random.randint(300,x_range-300)\n",
    "    y=np.random.randint(300,y_range-300)\n",
    "    return int(x),int(y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Robot_destination(Destination_x,Destination_y,Robot_x,Robot_y,Robot_a):\n",
    "    Delta_x=Destination_x-Robot_x\n",
    "    Delta_y=Destination_y-Robot_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Robot_a-Direct_angle)%360\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash\n",
    "# Is_Crash(10,10,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_arrive(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,30,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "# print(Is_arrive(1750,500,im4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Random_start(x_range,y_range,im):\n",
    "    a=0\n",
    "    x=np.random.random()*x_range\n",
    "    y=np.random.random()*y_range\n",
    "    while(Is_Crash(x,y,im)==True or Is_arrive(x,y,im)==True):\n",
    "        x=np.random.random()*x_range\n",
    "        y=np.random.random()*y_range\n",
    "    return x,y,a"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Height_level(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-60,-30,0,30,60]\n",
    "    Distance=np.arange(0,50,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "# Height_level(750,1000,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'L180D270/300/H1'"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def Output_state_index(True_angle,Left_right,Distance,Height):\n",
    "    if 0<=Distance<60:\n",
    "        Dis_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Dis_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Dis_level=2\n",
    "    else:\n",
    "        Dis_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    if Left_right=='right':\n",
    "        Le_ri=1\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Le_ri=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+Angle_level*4)\n",
    "    return State_number,Dis_level,Angle_level\n",
    "A,B,C=Output_state_index(80,'right',110,1)\n",
    "States[A]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Choose_action(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Length_action)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Next_state_F(Robot_x,Robot_y,Robot_a,Destination_x,Destination_y,Action,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Current_height=Height_level(Robot_x,Robot_y,Robot_a,im)\n",
    "    Action_level=0\n",
    "    \n",
    "    if Action==0:\n",
    "        Robot_next_x=Robot_x+Velocity_tripod*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_tripod*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==1:\n",
    "        Robot_next_x=Robot_x+Velocity_quadruped*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_quadruped*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==2:\n",
    "        Robot_next_x=Robot_x+Velocity_onebyone*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_onebyone*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==3:\n",
    "        Robot_a=Robot_a-45\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "    elif Action==4:\n",
    "        Robot_a=Robot_a+45\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "        \n",
    "   \n",
    "    \n",
    "    Next_true_angle,Next_le_ri,Next_distance=Robot_destination(Destination_x,Destination_y,Robot_next_x,Robot_next_y,Robot_a)\n",
    "    Next_height=Height_level(Robot_next_x,Robot_next_y,Robot_a,im)\n",
    "    Next_state_number,Next_distance_level,Next_angle_level=Output_state_index(Next_true_angle,Next_le_ri,Next_distance,Next_height)\n",
    "    \n",
    "    if (Current_height==2 and Action==0) or (Current_height==2 and Action==1):\n",
    "        Reward=-2\n",
    "    elif(Current_height==1 and Action==0):\n",
    "        Reward=-2\n",
    "    else: \n",
    "        if(Current_height==1 and Action==2):\n",
    "            Action_level=0.01\n",
    "        elif(Current_height==0 and Action==1) or (Current_height==0 and Action==2):\n",
    "            Action_level=0.01\n",
    "        elif(Action==0 or Action==1 or Action==2):\n",
    "            Action_level=0.04\n",
    "        else:\n",
    "            Action_level=0\n",
    "        if Arrive==False:\n",
    "            Reward=((3-Next_distance_level)*0.5+(6-Next_angle_level)+Action_level)*0.01\n",
    "        elif Arrive==True:\n",
    "            if(Current_height==1 and Action==2):\n",
    "                Reward=2\n",
    "            elif(Current_height==0 and Action==1) or (Current_height==0 and Action==2):\n",
    "                Reward=2\n",
    "            else:\n",
    "                Reward=6\n",
    "    return Robot_next_x,Robot_next_y,Robot_a,Reward,Next_state_number,Arrive,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-Table!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table=Initial_Q_Table(Length_action,Length_state)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times\n",
    "Action_times=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAmkAAAGXCAYAAADoAccmAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x1de7a506470>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************\n",
      "Destination_x=653.000000 Destination_y=1508.000000\n",
      "last_x=610.544783, last_y=1532.189632,last_a=0.000000\n",
      "Next_state_number=4 Reward=6.000000\n",
      "Next_x=622.104783 Next_y=1532.189632 True_angle=38.059424 DestinationRobotDistance=39.238409 Action=0 State=L60D30/60/H0\n",
      "DestinationRobotAngle=-38.059424 Robot_current_angle=0\n",
      "Epsilon=0.996752 Action_times=32865 Epoche_false=0\n",
      "**********************************\n",
      "Destination_x=1415.000000 Destination_y=1412.000000\n",
      "last_x=1445.855752, last_y=1376.166850,last_a=135.000000\n",
      "Next_state_number=96 Reward=6.000000\n",
      "Next_x=1443.140462 Next_y=1378.882140 True_angle=4.645226 DestinationRobotDistance=43.458926 Action=2 State=L60D0/30/H2\n",
      "DestinationRobotAngle=-49.645226 Robot_current_angle=135\n",
      "Epsilon=0.996579 Action_times=34613 Epoche_false=1\n",
      "**********************************\n",
      "Destination_x=1964.000000 Destination_y=850.000000\n",
      "last_x=1937.948726, last_y=807.873895,last_a=90.000000\n",
      "Next_state_number=52 Reward=-2.000000\n",
      "Next_x=1937.948726 Next_y=819.433895 True_angle=40.440719 DestinationRobotDistance=40.161619 Action=0 State=L60D30/60/H1\n",
      "DestinationRobotAngle=49.559281 Robot_current_angle=90\n",
      "Epsilon=0.995816 Action_times=42352 Epoche_false=2\n",
      "**********************************\n",
      "Destination_x=2153.000000 Destination_y=783.000000\n",
      "last_x=2145.886413, last_y=731.958740,last_a=90.000000\n",
      "Next_state_number=0 Reward=6.000000\n",
      "Next_x=2145.886413 Next_y=743.518740 True_angle=10.213757 DestinationRobotDistance=40.116991 Action=0 State=L60D0/30/H0\n",
      "DestinationRobotAngle=79.786243 Robot_current_angle=90\n",
      "Epsilon=0.994681 Action_times=53875 Epoche_false=3\n",
      "**********************************\n",
      "Destination_x=872.000000 Destination_y=1590.000000\n",
      "last_x=905.999058, last_y=1628.506189,last_a=225.000000\n",
      "Next_state_number=140 Reward=-2.000000\n",
      "Next_x=897.824903 Next_y=1620.332035 True_angle=4.588699 DestinationRobotDistance=39.836641 Action=0 State=L60D330/360/H2\n",
      "DestinationRobotAngle=49.588699 Robot_current_angle=225\n",
      "Epsilon=0.992777 Action_times=73229 Epoche_false=4\n",
      "**********************************\n",
      "Destination_x=675.000000 Destination_y=699.000000\n",
      "last_x=628.335601, last_y=704.844894,last_a=0.000000\n",
      "Next_state_number=0 Reward=2.000000\n",
      "Next_x=632.935601 Next_y=704.844894 True_angle=7.910659 DestinationRobotDistance=42.468535 Action=1 State=L60D0/30/H0\n",
      "DestinationRobotAngle=-7.910659 Robot_current_angle=0\n",
      "Epsilon=0.991642 Action_times=84785 Epoche_false=5\n"
     ]
    }
   ],
   "source": [
    "def Run_function():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Vector_x\n",
    "    global Vector_y\n",
    "    Crash_=False\n",
    "    Arrive_=False\n",
    "    Destination_x,Destination_y=Set_destination(3000,2000)\n",
    "    im4=Draw_map4(Destination_x,Destination_y)\n",
    "    plt.imshow(im4)\n",
    "    plt.show()\n",
    "    Current_x,Current_y,Current_a=Random_start(1000,2000,im4)\n",
    "    Epoche_false=0\n",
    "#     Vector_x=[]\n",
    "#     Vector_y=[]\n",
    "#     Vector_x.append(Current_x)\n",
    "#     Vector_y.append(Current_y)\n",
    "    while Epoche_false<500:\n",
    "\n",
    "        True_a,Le_ri,Distance=Robot_destination(Destination_x,Destination_y,Current_x,Current_y,Current_a)\n",
    "        Height_level_=Height_level(Current_x,Current_y,Current_a,im4)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_a,Le_ri,Distance,Height_level_)\n",
    "        Next_action=Choose_action(Q_Table,Current_state_number,Action_times)\n",
    "        Next_x,Next_y,Next_a,Reward,Next_state_number,Arrive,Crash=Next_state_F(Current_x,Current_y,Current_a,Destination_x,Destination_y,Next_action,im4)\n",
    "        if Crash==True:\n",
    "            Next_x,Next_y,Next_a=Random_start(3000,2000,im4)\n",
    "        else:\n",
    "            if Arrive==True:\n",
    "                Next_true_a,Next_lr,Next_distance=Robot_destination(Destination_x,Destination_y,Next_x,Next_y,Next_a)\n",
    "                print('**********************************')\n",
    "                print('Destination_x=%f Destination_y=%f'%(Destination_x,Destination_y))\n",
    "                print('last_x=%f, last_y=%f,last_a=%f'%(Current_x,Current_y,Current_a%360))\n",
    "                print('Next_state_number=%d Reward=%f'%(Next_state_number,Reward))\n",
    "                print('Next_x=%f Next_y=%f True_angle=%f DestinationRobotDistance=%f Action=%d State=%s'%(Next_x,Next_y,Next_true_a,Next_distance,Next_action,States[Next_state_number]))\n",
    "                print('DestinationRobotAngle=%f Robot_current_angle=%d'%((np.arctan((Next_y-Destination_y)/(Next_x-Destination_x)))*180/np.pi,Next_a%360))\n",
    "                print('Epsilon=%f Action_times=%d Epoche_false=%d'%(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times),Action_times,Epoche_false))\n",
    "                Destination_x,Destination_y=Set_destination(3000,2000)\n",
    "                im4=Draw_map4(Destination_x,Destination_y)\n",
    "#                 plt.imshow(im4)\n",
    "#                 plt.show()\n",
    "                Next_x,Next_y,Next_a=Random_start(3000,2000,im4)\n",
    "                Epoche_false+=1\n",
    "                Epoche+=1\n",
    "            Q_target=Reward+Beta*max(Q_Table[Next_state_number])\n",
    "            Q_Table[Current_state_number][Next_action]+=Alpha*(Q_target-Q_Table[Current_state_number][Next_action])\n",
    "#             Vector_x.append(Current_x)\n",
    "#             Vector_y.append(Current_y)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_a=Next_a\n",
    "        Action_times+=1\n",
    "    return Q_Table\n",
    "Q_Table_final=Run_function()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "Q_Table_=pd.DataFrame(Q_Table_final,columns=Actions,index=States)\n",
    "Q_Table_"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
